#include "wdtw.h"
#include <stdlib.h>
#include <math.h>
#include <float.h>

/*
 *  加权动态时间规整（WDTW）距离
 *  x, y          : 输入序列
 *  weight_vec    : y 各点的权重向量，长度 ≥ m
 *  sakoe_band    : Sakoe-Chiba 带状窗口半径
 *  alpha         : 权重指数，控制权重衰减
 *  返回值        :  WDTW 距离
 */
double wdtw(const double *x, int n,
            const double *y, int m,
            const double *weight_vec,
            int sakoe_band,
            double alpha)
{
    /* 若 sakoe_band 为负或足够大，则当作无约束 */
    if (sakoe_band < 0 || sakoe_band >= (n > m ? n : m))
        sakoe_band = (n > m ? n : m);   /* 等价于无窗口 */

    /* 分配并计算局部成本矩阵 dist[i][j] */
    double **dist = (double **)malloc(n * sizeof(double *));
    for (int i = 0; i < n; ++i)
        dist[i] = (double *)malloc(m * sizeof(double));

    for (int i = 0; i < n; ++i)
        for (int j = 0; j < m; ++j)
            dist[i][j] = (1.0 / pow(weight_vec[j], alpha)) * ((x[i] - y[j]) * (x[i] - y[j]));

    /* 分配 DTW 累积矩阵，初始化为无穷大 */
    double **dtw_cost = (double **)malloc(n * sizeof(double *));
    for (int i = 0; i < n; ++i)
    {
        dtw_cost[i] = (double *)malloc(m * sizeof(double));
        for (int j = 0; j < m; ++j)
            dtw_cost[i][j] = INFINITY;
    }
    /* 计算带有Sakoe-Chiba带的成本矩阵 */
    dtw_cost[0][0] = dist[0][0];

    for (int i = 1; i < n; ++i)
        if (abs(i - 0) <= sakoe_band)
            dtw_cost[i][0] = dtw_cost[i - 1][0] + dist[i][0];

    for (int j = 1; j < m; ++j)
        if (abs(0 - j) <= sakoe_band)
            dtw_cost[0][j] = dtw_cost[0][j - 1] + dist[0][j];

    for (int i = 1; i < n; ++i)
    {
        int j_start = (i - sakoe_band > 1) ? (i - sakoe_band) : 1;
        int j_end = (i + sakoe_band < m) ? (i + sakoe_band) : m - 1;
        for (int j = j_start; j <= j_end; ++j)
        {
            double min_prev = fmin(dtw_cost[i - 1][j],
                                   fmin(dtw_cost[i][j - 1],
                                        dtw_cost[i - 1][j - 1]));
            dtw_cost[i][j] = dist[i][j] + min_prev;
        }
    }

    double result = dtw_cost[n - 1][m - 1];

    /* 释放内存 */
    for (int i = 0; i < n; ++i)
    {
        free(dist[i]);
        free(dtw_cost[i]);
    }
    free(dist);
    free(dtw_cost);

    return result;
}
